from launch import LaunchDescription
from launch_ros.actions import Node
# 封装终端指令相关类--------------
# from launch.actions import ExecuteProcess
# from launch.substitutions import FindExecutable
# 参数声明与获取-----------------
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
# 文件包含相关-------------------
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
# 分组相关----------------------
# from launch_ros.actions import PushRosNamespace
# from launch.actions import GroupAction
# 事件相关----------------------
# from launch.event_handlers import OnProcessStart, OnProcessExit
# from launch.actions import ExecuteProcess, RegisterEventHandler,LogInfo
# 获取功能包下share目录路径-------
from ament_index_python.packages import get_package_share_directory
# import os
""" 
    需求：演示Node使用
    构造函数参数说明：

        :param: executable 可执行程序；T
        :param: package 被执行的程序所属的功能包；T
        :param: name 节点名称；T
        :param: namespace 设置命名空间；T
        :param: exec_name 设置程序标签；F
        :param: parameters 设置参数；F
        :param: remappings 话题重映射
        :param: ros_arguments 为节点传参
                            --ros-args xx yy
        :param: arguments 为节点传参
                            xx yy --ros-args

 """

def generate_launch_description():

         # 声明偏移参数（添加这些配置）
         offset_x_arg = DeclareLaunchArgument(
            'offset_x',
            default_value='0.0',
            description='X-axis position offset'
         )
         offset_y_arg = DeclareLaunchArgument(
            'offset_y',
            default_value='0.0',
            description='Y-axis position offset'
         )        
       # 声明一个名为child_link的启动参数，类型为字符串，默认值为空字符串
         child_frame_id = DeclareLaunchArgument(
                'child_frame_id',
                default_value='base_footprint',
                description='Value for the child_link parameter'
           )
         
         odomzuzhuang_dir = get_package_share_directory(
            'odom_zuzhuang')
         urdf2tf = IncludeLaunchDescription(
               PythonLaunchDescriptionSource(
               [odomzuzhuang_dir,'/urdf2tf.launch.py']),
           )
         
        #  pose_unstamper = Node(
        #    package="odom_zuzhuang",#要启动的节点
        #    executable="pose_unstamper",
        #    name="poseandtwist_unstamper"
        #    )#（功能包，可执行程序，名字）
        #  posewithcovariance = Node(
        #    package="odom_zuzhuang",#要启动的节点
        #    executable="posewithcovariance",
        #    name="pose_covariance"
        #    )
         odom = Node(
           package="odom_zuzhuang",#要启动的节点
           executable="odom_dds_2",
           name="odom_dds_2",
           parameters=[{
                  'frame_id': 'odom',
                  'child_frame_id': 'base_footprint',
                  # 添加偏移参数（通过LaunchConfiguration获取）
                  'offset_x': LaunchConfiguration('offset_x'),
                  'offset_y': LaunchConfiguration('offset_y'),
                  }  # 将启动参数传递给节点的参数
                ]
           )
         rename_imu = Node(
           package="odom_zuzhuang",#要启动的节点
           executable="rename_imu",
           name="rename_imu"
           )
         
         odom2tf = Node(
           package='odom_zuzhuang',
           executable='odom2tf',
           output='screen'
          )

         # 添加twist_stamper节点
         twist_stamper = Node(
            package='twist_stamper',  # 包名
            executable='twist_stamper',  # 可执行文件名
            name='twist_stamper',  # 节点名称
            remappings=[
                  ('cmd_vel_in', 'cmd_vel'),  # 重映射输入话题
                  ('cmd_vel_out', '/ap/cmd_vel')  # 重映射输出话题
            ],
             parameters=[{
                  'frame_id': 'base_link'  # 设置frame_id参数
            }]
            )

          # 添加USB摄像头节点
         usb_cam_node = Node(
                package='usb_cam',
                executable='usb_cam_node_exe',
                name='usb_cam_node',
                parameters=[{
                    'video_device': '/dev/video0',
                    'camera_name': 'default_cam',
                    'auto_white_balance': False,
                    'autoexposure': False,
                    'autofocus': False,
                    'enable_additional_controls': False,
                    'publish_camera_info': False
                }],
            )
         
         return LaunchDescription([
               # 添加偏移参数声明
                offset_x_arg,
                offset_y_arg,

                urdf2tf,
                # pose_unstamper,
                # posewithcovariance,
                odom,
                # rename_imu,
                odom2tf, 
                child_frame_id,
                twist_stamper,
                usb_cam_node,

               ])
    